/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.iaroc.irobot.sample;

import com.iaroc.irobot.*;
import com.sun.spot.sensorboard.EDemoBoard;
import com.sun.spot.util.Utils;

/**
 *
 * @author Mark
 */
public class OneMoreEventHandler extends IRobotCreateEventHandler 
    {
    //Data Members
    protected IRobotCreate create;
    protected EDemoBoard board;
    protected MaxSonarEZ1 sonar;
    protected int totalDistance;
    protected int spikeCount;
    protected boolean leftBump;
    protected boolean rightBump;
    protected boolean hit;
    // constants
    public final double MM_TO_FEET = 0.0032808399;
    public final double FEET_TO_MM = 1/MM_TO_FEET;
    
    public OneMoreEventHandler(IRobotCreate create, EDemoBoard board)
        {
        super();
        this.create = create;
        this.board = board;
        create.setVelocity(500);
        sonar = new MaxSonarEZ1(EDemoBoard.A0);
        rightBump = false;
        leftBump = false;
        hit = false;
        spikeCount = 0;
        totalDistance = 0;
        create.goForward();
        }

    public void bumpLeftEvent(boolean oldBoolean, boolean bumpLeft)
        {
        super.bumpLeftEvent(oldBoolean, bumpLeft);
        if (bumpLeft)
            {
            leftBump = bumpLeft;
            hit = false;
            if (hit)
                System.out.println("Bumper hit, stopping arc");
            }
        }

    public void bumpRightEvent(boolean oldBoolean, boolean bumpRight)
        {
        super.bumpRightEvent(oldBoolean, bumpRight);
        if (bumpRight)
            {
            rightBump = bumpRight;
            if (hit)
                System.out.println("Bumper hit, stopping arc");
            hit = false;
            }
        }

    public void eventBatchEnd() 
        {
        super.eventBatchEnd();
        if ((spikeCount > 1 || hit))
            {
            hit = true;
            spikeCount = 0;
            create.drive(500, 110);
            System.out.println("Arcing");
            }
        else
            {
            if (leftBump && rightBump)
                {
                create.setVelocity(100);
                create.spinRight();
                create.waitAngle(75);
                create.setVelocity(500);
                create.goForward();
                double distanceInFeet = totalDistance * MM_TO_FEET;
                System.out.println("Distance in feet: " + distanceInFeet);
                totalDistance = 0;
                }
            else
                {
                if (leftBump)
                    {
                    System.out.println("Left Bumper Hit");
                    create.goBackward();
                    create.waitDistance(100);
                    create.setVelocity(100);
                    create.spinRight();
                    create.waitAngle(6);
                    create.setVelocity(500);
                    create.goForward();
                    }
                if (rightBump)
                    {
                    System.out.println("Right Bumper Hit");
                    create.goBackward();
                    create.waitDistance(100);
                    create.setVelocity(100);
                    create.spinLeft();
                    create.waitAngle(6);
                    create.setVelocity(500);
                    create.goForward();
                    }
                if (!rightBump && !leftBump)
                    totalDistance += create.getDistance();
                }
            }
        }

    public void eventBatchStart()
        {
        super.eventBatchStart();
        rightBump = false;
        leftBump = false;
        }
    
    public void pollSensors()
        {
        create.sensors(IRobotCreateConstants.SENSORS_GROUP_ID6, this);
        spikeCount = sonar.getDistance() > 80? spikeCount + 1: 0;
        System.out.println("Sonar Distance: " + sonar.getDistance());
        }
    

    }//class FinalHomeBaseEventHandler